#include "Spring.h"
#include "Particle.h"
#include "MyGL.h"
#include <cassert>
#include <cmath>

namespace Engine
{
	
	void Spring::Set(float naturalLength, float kVal, float minDist, float maxDist, Particle* a, Particle* b)
	{
		m_naturalLength = naturalLength;
		m_kValue = kVal;
		m_minDist = minDist;
		m_maxDist = maxDist;
		m_a = a;
		m_b = b;
	}

	void Spring::Update()
	{
		assert(m_a != nullptr && m_b != nullptr);

		//work out distance between both particles
		Vec3 dist = (m_a ->GetPos() - m_b ->GetPos());
		float length = dist.Length();
		float x = length - m_naturalLength;
		
		//TODO
		//clamp force & dist

		//if x == ~0.0f return
		if(std::fabs(x) < 0.2f) return; 

		//hooke's law says f = -k x
		dist.Normalize();
		Vec3 force = (dist * (-m_kValue * x));

		//work out forces exerted on particles at both ends
		m_a ->AddForce(force);
		m_b ->AddForce(-force);
	}

	void Spring::Draw()
	{
		glBegin(GL_LINES);
			glColor3f(1.0f, 0.0f, 0.0f);
			glVertex3f(m_a ->GetPos().x, m_a ->GetPos().y, m_a ->GetPos().z);
			glColor3f(1.0f, 0.0f, 0.0f);
			glVertex3f(m_b ->GetPos().x, m_b ->GetPos().y, m_b ->GetPos().z);
		glEnd();
	}
}